#include "kalmanfilter.h"

// x=F*x
// P=F*P*Ft+Q
void KalmanFilter::Prediction() {
  // 状态转移矩阵 F_
  x_ = F_ * x_;
  Eigen::MatrixXd Ft = F_.transpose();
  // 状态协方差矩阵P_
  P_ = F_ * P_ * Ft + Q_;
}

// x=x+k*(z-H*x)
// P=(I-K*h)*P
// K=P*Ht*(H*P*Ht+R)t
void KalmanFilter::KFUpdate(Eigen::VectorXd z) {
  // 测量矩阵 H_  状态向量 x_
  Eigen::MatrixXd y = z - H_ * x_;

  Eigen::MatrixXd Ht = H_.transpose();

  // 测量噪声 R_
  Eigen::MatrixXd S = H_ * P_ * Ht + R_;

  Eigen::MatrixXd Si = S.inverse();
  // 状态协方差矩阵P_
  Eigen::MatrixXd K = P_ * Ht * Si;

  x_ = x_ + (K * y);
  int x_size = x_.size();
  Eigen::MatrixXd I = Eigen::MatrixXd::Identity(x_size, x_size);
  P_ = (I - K * H_) * P_;
}

// KF和EKF区别在于测量矩阵H的计算
// EKF的预测两个公式和之前一样，都是Prediction()函数，区别在于求解测量矩阵H矩阵。
// 首先是要求解测量值z和预测值x之间的偏差。y=z-Hx。
// 这里Hx用的就是将x从笛卡尔坐标系转化为极坐标系。
// 接下来的主要问题是在求解H矩阵。
void KalmanFilter::EKFUpdate(Eigen::VectorXd z) {
  double rho = sqrt(x_(0) * x_(0) + x_(1) * x_(1));
  double theta = atan2(x_(1), x_(0));
  double rho_dot = (x_(0) * x_(2) + x_(1) * x_(3)) / rho;
  Eigen::VectorXd h = Eigen::VectorXd(3);
  h << rho, theta, rho_dot;
  Eigen::VectorXd y = z - h;

  CalculateJacobianMatrix();

  Eigen::MatrixXd Ht = H_.transpose();
  Eigen::MatrixXd S = H_ * P_ * Ht + R_;
  Eigen::MatrixXd Si = S.inverse();
  Eigen::MatrixXd K = P_ * Ht * Si;
  x_ = x_ + (K * y);
  int x_size = x_.size();
  Eigen::MatrixXd I = Eigen::MatrixXd::Identity(x_size, x_size);
  P_ = (I - K * H_) * P_;
}

void KalmanFilter::CalculateJacobianMatrix() {
  Eigen::MatrixXd Hj(3, 4);

  // get state parameters
  float px = x_(0);
  float py = x_(1);
  float vx = x_(2);
  float vy = x_(3);

  // pre-compute a set of terms to avoid repeated calculation
  float c1 = px * px + py * py;
  float c2 = sqrt(c1);
  float c3 = (c1 * c2);

  // Check division by zero
  if (fabs(c1) < 0.0001) {
    H_ = Hj;
    return;
  }

  Hj << (px / c2), (py / c2), 0, 0, -(py / c1), (px / c1), 0, 0,
      py * (vx * py - vy * px) / c3, px * (px * vy - py * vx) / c3, px / c2,
      py / c2;
  H_ = Hj;
}
